#include <stdio.h>
#include <stdlib.h>
#include <memory.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "robot_control.h"
#include "hi_adc.h"
#include "iot_errno.h"
#include "iot_pwm.h"
#include "hi_pwm.h"
#include "hi_timer.h"

#define GPIO5 5
#define FUNC_GPIO 0
#define     ADC_TEST_LENGTH             (20)
#define     VLT_MIN                     (100)
#define     OLED_FALG_ON                ((unsigned char)0x01)
#define     OLED_FALG_OFF               ((unsigned char)0x00)

unsigned short  g_adc_buf[ADC_TEST_LENGTH] = { 0 };
unsigned short  g_gpio5_adc_buf[ADC_TEST_LENGTH] = { 0 };
unsigned int  g_gpio5_tick = 0;
unsigned char   g_car_control_mode = 0;
unsigned char   g_car_speed_control = 0;
unsigned int  g_car_control_demo_task_id = 0;
unsigned char   g_car_status = CAR_STOP_STATUS; // 初始停止状态，需要按键切换

// 连续PWM速度控制相关全局变量
unsigned char   g_current_speed_level = CAR_SPEED_LEVEL_0;  // 当前速度等级，从停止开始

// 显示速度等级（OLED显示用）
unsigned char   g_display_speed_levels[CAR_SPEED_MAX_LEVEL + 1] = {
    CAR_SPEED_LEVEL_0, CAR_SPEED_LEVEL_1, CAR_SPEED_LEVEL_2,
    CAR_SPEED_LEVEL_3, CAR_SPEED_LEVEL_4, CAR_SPEED_LEVEL_5
};

// 实际输出速度等级（电机控制用）- 进一步降低高速，防止飞出线
unsigned char   g_actual_speed_levels[CAR_SPEED_MAX_LEVEL + 1] = {
    0,   // 0% -> 0% (停止)
    50,  // 30% -> 50% (修正30%不动的问题)
    70,  // 50% -> 70%
    70,  // 70% -> 70% (保持不变)
    75,  // 90% -> 75% (进一步降低)
    80   // 100% -> 80% (进一步降低，防止飞出线)
};

unsigned char   g_current_level_index = 0;  // 当前速度等级索引，从0开始

// PWM占空比控制 - 连续速度控制
unsigned char   g_target_duty_cycle = CAR_SPEED_LEVEL_0;    // 目标占空比，从0开始
unsigned char   g_current_duty_cycle = 0;                  // 当前占空比
unsigned char   g_speed_transition_enabled = 1;            // 是否启用平滑过渡

/*
 * 函数：get_actual_speed_from_display
 * 功能：根据显示速度获取实际输出速度
 * 输入：display_speed - 显示的速度值
 * 输出：实际输出的速度值
 */
unsigned char get_actual_speed_from_display(unsigned char display_speed)
{
    // 根据显示速度查找对应的实际速度
    for (int i = 0; i <= CAR_SPEED_MAX_LEVEL; i++) {
        if (g_display_speed_levels[i] == display_speed) {
            return g_actual_speed_levels[i];
        }
    }

    // 如果没有找到精确匹配，根据范围映射（进一步降低高速）
    if (display_speed == 0) return 0;
    else if (display_speed <= 30) return 50;
    else if (display_speed <= 50) return 70;
    else if (display_speed <= 70) return 70;  // 70%显示速度映射到70%实际速度
    else if (display_speed <= 90) return 75;  // 90%显示速度映射到75%实际速度
    else return 80;  // 100%显示速度映射到80%实际速度
}

/*
 * 函数：get_display_speed
 * 功能：获取当前显示的速度值（用于OLED显示）
 * 输入：无
 * 输出：显示的速度值
 */
unsigned char get_display_speed(void)
{
    if (g_current_level_index <= CAR_SPEED_MAX_LEVEL) {
        return g_display_speed_levels[g_current_level_index];
    }
    return 0;
}

extern float GetDistance(void);
extern void trace_module(void);
extern void car_backward(void);
extern void car_forward(void);
extern void car_left(void);
extern void car_right(void);
extern void car_sharp_left(void);  // 急左转函数声明
extern void car_sharp_right(void); // 急右转函数声明
extern void car_stop(void);
extern void engine_turn_left(void);
extern void engine_turn_right(void);
extern void regress_middle(void);
extern void set_motor_speed_level(unsigned char speed_level);
extern unsigned char get_motor_speed_level(void);
extern void motor_pwm_control(void);
extern void reset_sharp_turn_boost(void);  // 重置急转弯提速函数声明


void switch_init(void)
{
    IoTGpioInit(5);
    hi_io_set_func(5, 0);
    IoTGpioSetDir(5, IOT_GPIO_DIR_IN);
    hi_io_set_pull(5, 1);
}

//按键状态管理变量 - 参考您的高效检测方法
static unsigned char g_key_val = 0;      // 当前按键状态 (逻辑值: 1-按下, 0-弹起)
static unsigned char g_key_old = 0;      // 上一次的按键状态
static unsigned char g_key_down = 0;     // 按下事件标志
static unsigned char g_key_up = 0;       // 弹起事件标志
static unsigned int g_key_press_start = 0;  // 按下开始时间
static unsigned char g_long_press_triggered = 0; // 长按是否已触发

//简化的按键中断响应函数 - 仅用于唤醒，实际检测在定时器中进行
void gpio5_isr_func_mode(void)
{
    // 简单的中断响应，主要检测在定时器中进行
    printf("Key interrupt triggered\n");
}

// 高效的按键检测函数 - 参考您的代码逻辑
void key_scan_timer_callback(unsigned int arg)
{
    static unsigned int long_press_counter = 0;

    // 1. 读取GPIO原始电平
    IotGpioValue gpio_val;
    IoTGpioGetInputVal(5, &gpio_val);

    // 2. 将物理电平转换为逻辑状态 (按下为低电平)
    g_key_val = (gpio_val == IOT_GPIO_VALUE0);
    g_key_down = g_key_val & (g_key_old ^ g_key_val);  // 捕获按下瞬间
    g_key_up = ~g_key_val & (g_key_old ^ g_key_val);   // 捕获释放瞬间
    g_key_old = g_key_val;

    // 3. 处理按下事件
    if (g_key_down) {
        g_key_press_start = hi_get_tick();
        long_press_counter = 0;
        g_long_press_triggered = 0;
        printf("Key pressed\n");
    }

    // 4. 处理长按检测 (按键持续按下时)
    if (g_key_val && !g_long_press_triggered) {
        long_press_counter++;
        // 长按检测：按下超过500ms (50次 * 10ms)
        if (long_press_counter > 50) {
            g_long_press_triggered = 1;
            car_speed_up();
            printf("Long press - Speed level: %d\n", g_current_level_index);
        }
    }

    // 5. 处理释放事件 (短按)
    if (g_key_up && !g_long_press_triggered) {
        // 短按：切换模式
        if (g_car_status == CAR_STOP_STATUS) {
            g_car_status = CAR_TRACE_STATUS;
            printf("*** SHORT PRESS - SWITCHED TO TRACE MODE ***\r\n");
            printf("Status changed: %d -> %d (Trace mode activated)\r\n", CAR_STOP_STATUS, CAR_TRACE_STATUS);
        } else if (g_car_status == CAR_TRACE_STATUS) {
            g_car_status = CAR_OBSTACLE_AVOIDANCE_STATUS;
            printf("*** SHORT PRESS - SWITCHED TO OBSTACLE AVOIDANCE MODE ***\r\n");
            printf("Status changed: %d -> %d (Obstacle avoidance activated)\r\n", CAR_TRACE_STATUS, CAR_OBSTACLE_AVOIDANCE_STATUS);
        } else if (g_car_status == CAR_OBSTACLE_AVOIDANCE_STATUS) {
            g_car_status = CAR_STOP_STATUS;
            printf("*** SHORT PRESS - SWITCHED TO STOP MODE ***\r\n");
            printf("Status changed: %d -> %d (Stop mode activated)\r\n", CAR_OBSTACLE_AVOIDANCE_STATUS, CAR_STOP_STATUS);
        }
    }
}

unsigned char get_gpio5_voltage(void *param)
{
    int i;
    unsigned short data;
    unsigned int ret;
    unsigned short vlt;
    float voltage;
    float vlt_max = 0;
    float vlt_min = VLT_MIN;

    hi_unref_param(param);
    memset_s(g_gpio5_adc_buf, sizeof(g_gpio5_adc_buf), 0x0, sizeof(g_gpio5_adc_buf));
    for (i = 0; i < ADC_TEST_LENGTH; i++) {
        ret = hi_adc_read(HI_ADC_CHANNEL_2, &data, HI_ADC_EQU_MODEL_4, HI_ADC_CUR_BAIS_DEFAULT, 0xF0); 
		//ADC_Channal_2  自动识别模式  CNcomment:4次平均算法模式 CNend */
        if (ret != IOT_SUCCESS) {
            printf("ADC Read Fail\n");
            return  NULL;
        }    
        g_gpio5_adc_buf[i] = data;
    }

    for (i = 0; i < ADC_TEST_LENGTH; i++) {  
        vlt = g_gpio5_adc_buf[i]; 
        voltage = (float)vlt * 1.8 * 4 / 4096.0;  
		/* vlt * 1.8* 4 / 4096.0为将码字转换为电压 */
        vlt_max = (voltage > vlt_max) ? voltage : vlt_max;
        vlt_min = (voltage < vlt_min) ? voltage : vlt_min;
    }
    printf("vlt_max is %f\r\n", vlt_max);
    if (vlt_max > 0.6 && vlt_max < 1.0) {
        gpio5_isr_func_mode();
    } 
}



//按键中断
void interrupt_monitor(void)
{
    unsigned int  ret = 0;
    /*gpio5 switch2 mode*/
    g_gpio5_tick = hi_get_tick();
    ret = IoTGpioRegisterIsrFunc(GPIO5, IOT_INT_TYPE_EDGE, IOT_GPIO_EDGE_FALL_LEVEL_LOW, get_gpio5_voltage, NULL);
    if (ret == IOT_SUCCESS) {
        printf(" register gpio5\r\n");
    }
}

/*Judge steering gear*/
static unsigned int engine_go_where(void)
{
    float left_distance = 0;
    float right_distance = 0;
    /*舵机往左转动测量左边障碍物的距离*/

    engine_turn_left();
    hi_sleep(100);
    left_distance = GetDistance();
    hi_sleep(100);

    /*归中*/
    regress_middle();
    hi_sleep(100);

    /*舵机往右转动测量右边障碍物的距离*/
    engine_turn_right();
    hi_sleep(100);
    right_distance = GetDistance();
    hi_sleep(100);

    /*归中*/
    regress_middle();
    if (left_distance > right_distance) {
        return CAR_TURN_LEFT;
    } else {
        return CAR_TURN_RIGHT;
    }
}

/*根据障碍物的距离来判断小车的行走方向
1、距离大于等于20cm继续前进
2、距离小于20cm，先停止再后退0.5s,再继续进行测距,再进行判断
*/
/*Judge the direction of the car*/
static void car_where_to_go(float distance)
{
    if (distance < DISTANCE_BETWEEN_CAR_AND_OBSTACLE) {
        car_stop();
        hi_sleep(500);
        car_backward();
        hi_sleep(500);
        car_stop();
        unsigned int ret = engine_go_where();
        printf("ret is %d\r\n", ret);
        if (ret == CAR_TURN_LEFT) {
            car_left();
            hi_sleep(500);
        } else if (ret == CAR_TURN_RIGHT) {
            car_right();
            hi_sleep(500);
        }
        car_stop();
    } else {
        car_forward();
        } 
}

/*car mode control func*/
static void car_mode_control_func(void)
{
    float m_distance = 0.0;
    regress_middle();
    while (1) {
        if (g_car_status != CAR_OBSTACLE_AVOIDANCE_STATUS) {
            printf("car_mode_control_func 1 module changed\n");
            regress_middle();
            break;
        }

        /*获取前方物体的距离*/
        m_distance = GetDistance();
        // printf("m_distance = %f\n",m_distance);
        car_where_to_go(m_distance);
        hi_sleep(20);
    }
}

void *RobotCarTestTask(void* param)
{
	printf("=== Robot Car System Starting ===\r\n");
	printf("Current status: %d (0=Stop, 1=Obstacle, 2=Trace)\r\n", g_car_status);
	printf("Press button to switch modes\r\n");
	printf("Gray sensor uses GPIO11(CLK) and GPIO12(DAT)\r\n");
    switch_init();
    interrupt_monitor();

    // 创建400Hz PWM控制定时器 - 调整定时器间隔
    unsigned int motor_timer_id;
    hi_timer_create(&motor_timer_id);
    hi_timer_start(motor_timer_id, HI_TIMER_TYPE_PERIOD, 1, motor_pwm_control, 0);  // 1ms定时器配合分辨率调整

    // 创建按键扫描定时器 - 10ms扫描周期
    unsigned int key_timer_id;
    hi_timer_create(&key_timer_id);
    hi_timer_start(key_timer_id, HI_TIMER_TYPE_PERIOD, 10, key_scan_timer_callback, 0);

    while (1) {
        switch (g_car_status) {
            case CAR_STOP_STATUS:
                car_stop();
                break;
            case CAR_OBSTACLE_AVOIDANCE_STATUS:
                car_mode_control_func();
                break;
            case CAR_TRACE_STATUS:
                trace_module();
                break;
            default:
                break;
        }
        IoTWatchDogDisable();
        osDelay(20);
    }
}

void RobotCarDemo(void)
{
    osThreadAttr_t attr;

    // 初始化速度控制
    car_speed_init();

    attr.name = "RobotCarTestTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 10240;
    attr.priority = 22;

    if (osThreadNew(RobotCarTestTask, NULL, &attr) == NULL) {
        printf("[Ssd1306TestDemo] Falied to create RobotCarTestTask!\n");
    }
}

// 速度控制相关函数实现

// 速度控制初始化函数
void car_speed_init(void)
{
    // 设置初始速度
    unsigned char actual_speed = get_actual_speed_from_display(g_current_speed_level);
    set_motor_speed_level(actual_speed);
    printf("Car speed control initialized - Display: %d%%, Actual: %d%%\n",
           get_display_speed(), actual_speed);
}

// 速度增加函数 - 连续PWM控制（循环模式）
void car_speed_up(void)
{
    if (g_current_level_index < CAR_SPEED_MAX_LEVEL) {
        g_current_level_index++;
    } else {
        // 达到最大等级时循环回到0
        g_current_level_index = 0;
    }

    // 获取显示速度和实际速度
    unsigned char display_speed = get_display_speed();
    unsigned char actual_speed = get_actual_speed_from_display(display_speed);

    g_current_speed_level = display_speed;  // 保存显示速度
    g_target_duty_cycle = actual_speed;     // 使用实际速度作为目标占空比
    set_motor_speed_level(actual_speed);    // 设置实际速度到电机

    printf("Speed cycle to level %d - Display: %d%%, Actual: %d%%\n",
           g_current_level_index, display_speed, actual_speed);
}

// 速度减少函数 - 连续PWM控制
void car_speed_down(void)
{
    if (g_current_level_index > 0) {
        g_current_level_index--;

        // 获取显示速度和实际速度
        unsigned char display_speed = get_display_speed();
        unsigned char actual_speed = get_actual_speed_from_display(display_speed);

        g_current_speed_level = display_speed;  // 保存显示速度
        g_target_duty_cycle = actual_speed;     // 使用实际速度作为目标占空比
        set_motor_speed_level(actual_speed);    // 设置实际速度到电机

        printf("Speed down to level %d - Display: %d%%, Actual: %d%%\n",
               g_current_level_index, display_speed, actual_speed);
    } else {
        printf("Already at minimum speed level\n");
    }
}

// 获取当前速度等级（显示用）
unsigned char get_current_speed_level(void)
{
    return get_display_speed();  // 返回显示速度，用于OLED显示
}

// 获取当前实际速度等级（电机控制用）
unsigned char get_current_actual_speed_level(void)
{
    return get_actual_speed_from_display(get_display_speed());
}

// 获取当前速度对应的延时
unsigned int get_current_speed_delay(void)
{
    // 根据速度等级返回不同的基础延时
    switch(g_current_level_index) {
        case 0: return 0;     // 停止
        case 1: return 100;   // 低速：较长延时
        case 2: return 50;    // 中低速
        case 3: return 30;    // 中高速
        case 4: return 20;    // 高速：较短延时
        default: return 50;
    }
}

// 设置电机速度（简化版本）
void set_motor_speed(unsigned char left_speed, unsigned char right_speed)
{
    // 计算平均速度
    unsigned char avg_speed = (left_speed + right_speed) / 2;

    // 应用速度映射
    unsigned char actual_speed = get_actual_speed_from_display(avg_speed);

    // 这个函数现在只是为了兼容性，实际速度控制在robot_l9110s.c中实现
    set_motor_speed_level(actual_speed);

    printf("set_motor_speed: Display: %d%%, Actual: %d%%\n", avg_speed, actual_speed);
}

APP_FEATURE_INIT(RobotCarDemo);